#include <tf2/LinearMath/Quaternion.h>

#include <rclcpp/create_timer.hpp>
#include <rclcpp/logging.hpp>
#include <rclcpp/qos.hpp>
#include <rclcpp/utilities.hpp>
#include <serial_driver/serial_driver.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
// C++ system
#include <cstdint>
#include <functional>
#include <future>
#include <map>
#include <memory>
#include <string>
#include <vector>

#include "rclcpp/rclcpp.hpp"
// 引用头文件
#include "rm_fake_serial_driver/rm_fake_serial_driver.hpp"
// 从vision_interfaces加载消息类型
#include "vision_interfaces/msg/robot.hpp"  // vision_interfaces::msg::Robot 类型

namespace rm_fake_serial_driver
{
FakeSerialDriverNode::FakeSerialDriverNode(const rclcpp::NodeOptions & options)
: Node("rm_fake_serial_driver_node", options),
  tf_broadcaster_(std::make_unique<tf2_ros::TransformBroadcaster>(*this))  // 传递节点名称
{
  RCLCPP_INFO(get_logger(), "Start RMFakeSerialDriver!");
  //timestamp_offset_ = this->declare_parameter("timestamp_offset", 0.0);

  // Declare parameters from YAML file
  this->declare_parameter("roll", 0.0);
  this->declare_parameter("pitch", 0.0);
  this->declare_parameter("yaw", 0.0);

  // 创建一个发布者，用于发布Robot类型的消息
  robot_pub_ =
    this->create_publisher<vision_interfaces::msg::Robot>("serial_driver/robotgimbal", 10);
  //debug
  latency_pub_ = this->create_publisher<std_msgs::msg::Float64>("latency", 10);
  marker_pub_ = this->create_publisher<visualization_msgs::msg::Marker>("aiming_point", 10);

  // 使用异步回调来替代定时器
  // async表示使用独立的线程中运行timerTask函数
  timer_future_ = std::async(std::launch::async, &FakeSerialDriverNode::timerTask, this);
  // // rclcpp::AsyncParametersClient 是 ROS 2 中的一个类，用于异步地与参数服务器进行通信
  // // 当前对象将与名为 "armor_detector" 的参数服务器进行通信
  // detector_param_client_ = std::make_shared<rclcpp::AsyncParametersClient>(this, "armor_detector");

  // // Tracker reset service client
  // reset_tracker_client_ = this->create_client<std_srvs::srv::Trigger>("tracker/reset");
  // resetTracker();
}

void FakeSerialDriverNode::timerTask()
{
  while (rclcpp::ok()) {
    std::this_thread::sleep_for(std::chrono::seconds(1));
    publishOdomToGimbalTransform();
  }
}

void FakeSerialDriverNode::publishOdomToGimbalTransform()
{
  // RCLCPP_INFO(this->get_logger(), "publishOdomToGimbalTransform called");

  try {
    // 获取来自参数服务器的roll, pitch, yaw值
    double roll, pitch, yaw;
    this->get_parameter("roll", roll);
    this->get_parameter("pitch", pitch);
    this->get_parameter("yaw", yaw);

    // 构造从 "odom" 到 "gimbal_link" 的变换
    geometry_msgs::msg::TransformStamped transform;

    // 设置消息头
    transform.header.stamp = this->get_clock()->now();
    transform.header.frame_id = "odom";        // 变换从 "odom" 坐标系发布
    transform.child_frame_id = "gimbal_link";  // 到 "gimbal_link" 坐标系

    // 假设 "odom" 到 "gimbal_link" 的变换为一个旋转矩阵
    tf2::Quaternion quat;
    quat.setRPY(roll, pitch, yaw);  // 设置 roll=0, pitch=0, yaw=0度

    // 将四元数存入变换中；setRPY方法用于将欧拉角转换为四元数
    transform.transform.rotation = tf2::toMsg(quat);

    // 将变换广播出去
    tf_broadcaster_->sendTransform(transform);

    // 将四元数转换为欧拉角
    tf2::Matrix3x3 mat(quat);
    double robot_roll, robot_pitch, robot_yaw;
    mat.getRPY(robot_roll, robot_pitch, robot_yaw);

    // RCLCPP_INFO(this->get_logger(), "tf send success");
    // RCLCPP_INFO(this->get_logger(), "Pitch: %f, Yaw: %f", robot_pitch, robot_yaw);

    // Publish the robot message
    robot_msg.self_pitch = robot_pitch * 180 / M_PI;
    robot_msg.self_yaw = robot_yaw * 180 / M_PI;
    robot_pub_->publish(robot_msg);

  } catch (tf2::TransformException & ex) {
    RCLCPP_ERROR(this->get_logger(), "Transform error: %s", ex.what());
  }
}

void FakeSerialDriverNode::resetTracker()
{
  if (!reset_tracker_client_->service_is_ready()) {
    RCLCPP_WARN(get_logger(), "Service not ready, skipping tracker reset");
    return;
  }

  auto request = std::make_shared<std_srvs::srv::Trigger::Request>();
  reset_tracker_client_->async_send_request(request);
  RCLCPP_INFO(get_logger(), "Reset tracker!");
}

FakeSerialDriverNode::~FakeSerialDriverNode()
{
  if (timer_future_.valid()) {
    timer_future_.wait();
  }
}

}  // namespace rm_fake_serial_driver

#include "rclcpp_components/register_node_macro.hpp"

// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE(rm_fake_serial_driver::FakeSerialDriverNode)